
import lejos.nxt.LCD;
import lejos.nxt.comm.RConsole;
import lejos.robotics.subsumption.Behavior;
import lejos.util.Delay;
import lejos.util.Stopwatch;

import java.lang.Math;
public class MoveAwayFromWall extends RobotMoving implements Behavior {

	@Override
	public void action() {
		
		RConsole.println("Too Close to Wall!.");
		pilot.rotate(-25);
		pilot.travel(10);
	}

	@Override
	public void suppress() {
		if (pilot.isMoving()) {
			pilot.stop();
		}
	}

	@Override
	public boolean takeControl() {
		// We want to take control if we are now facing close enough to a wall
		Delay.msDelay(0);
		int distance = wallSensor.getDistance();
		
		if(distance <= 12 && distance > 4) {
			return true;
		}
		return false;

	}

}
